System and method for lane boundary estimation and host vehicle position and orientation

ABSTRACT

Lane Boundary Estimation and Host Vehicle Position and Orientation, within the host lane estimation, using V2V (vehicle to vehicle) system, are discussed here. Lane boundary detection and tracking is essential for many active safety/ADAS application. The lane boundary position enables the tracking of the host vehicle position and orientation inside the lane. It also enables classifying in-lane, adjacent lanes, and other lanes vehicles. These two functionalities (lane boundary estimation and vehicle lane classifications) enable active safety applications (such as LDW, FCW, ACC, or BSD). It also enables the lateral control of the vehicle for lane keeping assist system, or for full lateral control for automated vehicle (automated for one or multiple lane changes).

RELATED APPLICATION

We have filed another related application earlier, titled “System andmethod for node adaptive filtering and congestion control for safety andmobility applications toward automated vehicles system”, copending nowat the USPTO, with the same inventor(s) and assignee, and a relatedsubject matter. We incorporate all the teaching of the prior applicationabove, by reference, including any Appendix or figures.

BACKGROUND OF THE INVENTION

The present invention relates to a system that uses the Vehicle toVehicle and/or the Vehicle to infrastructure communication for safetyand mobility applications. The invention provides methods for laneboundary estimation and even some LDW functionality using V2V and/or V2Isystems.

Dedicated Short Range Communication (DSRC) is the main enablingtechnology for connected vehicle applications that will reduce vehiclecrashes through fully connected transportation system with integratedwireless devices and road infrastructure. In such connected system, dataamong vehicles and with road infrastructure will be exchanged withacceptable time delay. DSRC is the enabler for the V2X communication andprovides 360 degrees field of view with long rangedetection/communication capability up to 1000 meter. Data such asvehicle position, dynamics and signals can be exchanged among vehiclesand road side equipments, which make the deployment of safetyapplications, such as crash avoidance systems (warning and control),possible. V2X technology will complement and get fused with the currentproduction crash avoidance technologies that use radar and visionsensing. V2V will give drivers information needed for safer driving(driver makes safe decisions) on the road that radar and vision systemscannot provide. This V2X capability, therefore, offers enhancements tothe current production crash avoidance systems, and also enablesaddressing more complex crash scenarios, such as those occurring atintersections. This kind of integration between the current productioncrash avoidance systems, V2X technology, and other transportationinfrastructure paves the way for realizing automated vehicles system.

The safety, health, and cost of accidents (on both humans andproperties) are major concerns for all citizens, local and Federalgovernments, cities, insurance companies (both for vehicles and humans),health organizations, and the Congress (especially due to the budgetcuts, in every level). People inherently make a lot of mistakes duringdriving (and cause accidents), due to the lack of sleep, variousdistractions, talking to others in the vehicle, fast driving, longdriving, heavy traffic, rain, snow, fog, ice, or too much drinking. Ifwe can make the driving more automated by implementing different scaleof safety applications and even controlling the motion of the vehiclefor longer period of driving, that saves many lives and potentiallybillions of dollars each year, in US and other countries. We introducehere an automated vehicle infrastructure and control systems andmethods. That is the category of which the current invention is under,where V2X communication technology is vital component of such system,with all the embodiments presented here and in the divisional cases, inthis family.

SUMMARY OF THE INVENTION

Lane Boundary Estimation and Host Vehicle Position and Orientation,within the host lane estimation, using V2V (vehicle to vehicle) and/orV2I (vehicle to infrastructure) system, are presented here. Laneboundary detection and tracking is essential for many active safety/ADASapplication. It is also very essential for any level of automatedsystem. The lane boundary position enables the tracking of the hostvehicle position and orientation inside the lane. It also enablesclassifying in-lane, adjacent lanes, and other lanes vehicles. These twofunctionalities (lane boundary estimation and vehicle laneclassifications) enable active safety applications (such as LDW, FCW,ACC, or BSD). It also enables the lateral control of the vehicle forlane keeping assist system, or for full lateral control for automatedvehicle (automated for one or multiple lane changes). Currenttechnologies for lane boundary detection and tracking are mainlyvision-based.

An embodiment for this invention is a method for lane boundaryestimation, and even some LDW functionality, using V2V and or V2Isystem. Some of the features of this embodiment are due to thefollowing:

1—In an automated system, it will be very difficult to detect and trackall lane boundaries using a vision system, due to multiple reasons:limited Field of View (FOV) coverage, difficulty seeing lane marking inhigh traffic scenario, or challenges facing vision system in differentenvironment conditions (poor lane marking, challenging weather, such asice, snow, or leaves, challenging lighting conditions, upcoming curvesat nights, or the like).

2—Poor availability of LDW system in the above conditions, stated insection 1.

3—V2V active safety systems/ADAS are for vehicle to vehicle threat type,and not intended for road attribute threat type, such as drifting awayin your lane, as in LDW system. Therefore, having such system using V2Vonly may save a vision system cost for lane boundary detection and/orLDW.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is for one embodiment, as an example, for representation ofdevelopment of fully automated vehicles, in stages.

FIG. 2 is for one embodiment of the invention, for a system forautomated vehicles.

FIG. 3 is for one embodiment of the invention, for a system forautomated vehicles.

FIG. 4 is for one embodiment of the invention, for automated vehiclefunctional architecture.

FIG. 5 is for one embodiment of the invention, for automated vehicleinfrastructure architecture.

FIG. 6 is for one embodiment of the invention, for a system for V2Xlandscape, with components.

FIG. 7 is for one embodiment of the invention, for a system forframework for V2I applications, with components.

FIG. 8 is for one embodiment of the invention, for a system forautomated vehicle command and control (C2) cloud, with components.

FIG. 9 is for one embodiment of the invention, for a system for SavariC2 network, with components, showing communications between networks andvehicles.

FIG. 10 is for one embodiment of the invention, for a system for hostvehicle, range of R values, region(s) defined, multiple nodes orvehicles inside and outside region(s), for communications betweennetworks and vehicles, and warning decisions or filtering purposes.

FIG. 11 is for one embodiment of the invention, for a system for hostvehicle, range of R values, region(s) defined, for an irregularshape(s), depending on (x,y) coordinates in 2D (dimensional)coordinates, defining the boundaries.

FIG. 12 is for one embodiment of the invention related to virtualboundaries and clustering vehicles.

FIG. 13 is for one embodiment of the invention related to current andhistory of data for vehicles.

FIG. 14 is for one embodiment of the invention related to clustering,distances between clusters, and statistical distributions for vehicles.

FIG. 15 is for one embodiment of the invention, for a system for lanedetermination.

FIG. 16 is for one embodiment of the invention, for a system forclustering.

FIG. 17 is for one embodiment of the invention, for a system forclustering and cluster analysis.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

FIG. 1 is for one embodiment, as an example, for representation ofdevelopment of fully automated vehicles, in stages, for progressiontoward fully automated vehicles. FIG. 2 is for one embodiment of theinvention, for a system for automated vehicles, using GPS, independentsensors, and maps, for vehicle interactions, driving dynamics, andsensor fusions and integrations.

FIG. 3 is for one embodiment of the invention, for a system forautomated vehicles, with different measurement devices, e.g., LIDAR(using laser, scanner/optics, photodetectors/sensors, andGPS/position/navigation systems, for measuring the distances, based ontravel time for light), radar, GPS, traffic data, sensors data, orvideo, to measure or find positions, coordinates, and distances. Thegovernment agencies may impose restrictions on security and encryptionof the communications and data for modules and devices within thesystem, as the minimum requirements, as the hackers or terrorists maytry to get into the system and control the vehicles for a destructivepurpose. Thus, all of our components are based on those requirementsimposed by the US or other foreign governments, to comply with thepublic safety.

FIG. 4 is for one embodiment of the invention, for automated vehiclefunctional architecture, for sensing, perception, applications, andactuation. FIG. 5 is for one embodiment of the invention, for automatedvehicle infrastructure architecture, for sensing, gateway, and services.

FIG. 6 is for one embodiment of the invention, for a system for V2Xlandscape, with components, for spectrum and range of frequencies andcommunications, for various technologies, for various purposes, fordifferent ranges. FIG. 7 is for one embodiment of the invention, for asystem for framework for V2I applications, with components, forroad-side platform and on-board platform, using various messages andsensors.

FIG. 8 is for one embodiment of the invention, for a system forautomated vehicle command and control (C2) cloud, with components, withvarious groups and people involved, as user, beneficiary, oradministrator. FIG. 9 is for one embodiment of the invention, for asystem for Savari C2 network, with components, showing communicationsbetween networks and vehicles, using traffic centers' data andregulations by different government agencies.

FIG. 10 is for one embodiment of the invention, for a system for hostvehicle, range of R values, region(s) defined, multiple nodes orvehicles inside and outside region(s), for communications betweennetworks and vehicles, and warning decisions or filtering purposes, forvarious filters to reduce computations and reduce the bandwidth neededto handle the message traffic. FIG. 11 is for one embodiment of theinvention, for a system for host vehicle, range of R values, region(s)defined, for an irregular shape(s), depending on (x,y) coordinates in 2D(dimensional) coordinates, defining the boundaries, or in 3D forcrossing highways in different heights, if connecting.

In one embodiment, we have the following technical components for thesystem: vehicle, roadway, communications, architecture, cybersecurity,safety reliability, human factors, and operations. In one embodiment, wehave the following non-technical analysis for the system: public policy,market evolution, legal/liability, consumer acceptance, cost-benefitanalysis, human factors, certification, and licensing.

In one embodiment, we have the following requirements for AV (automatedvehicles) system:

-   -   Secure reliable connection to the command and control center    -   Built-in fail-safe mechanisms    -   Knowledge of its position and map database information (micro        and macro maps)    -   Communication with traffic lights/road side infrastructure    -   Fast, reliable, and secure    -   Situational awareness to completely understand its immediate        surrounding environment    -   Requires multiple sensors    -   Algorithms to analyze information from sensors    -   Algorithms to control the car, for drive-by-wire capability

In one embodiment, we have the following primary technologies for oursystem:

-   -   V2X communication: time-critical and reliable, secure, cheap,        and dedicated wireless spectrum    -   Car OBE (on-board equipment): sensor integration (vision, radar        and ADAS (advanced driver assistance system)), positioning        (accurate position, path, local map), wireless module (physical        layer (PHY), Media Access Control (MAC), antenna), security        (multi-layer architecture), processing and message engine, and        algorithms for vehicle prediction and control

In one embodiment, we have the following building blocks for AVs:

-   -   Automation Platform        -   i. Advanced Driver Assistance (ADAS) integration        -   ii. Map Integration, Lane Control        -   iii. Radio communications support        -   iv. Vehicle Controller Unit to do actuation    -   Base Station        -   Ground positioning support to improve positioning accuracy        -   V2I (vehicle to infrastructure) functionality, support for            public/private spectrums        -   Cloud connectivity to provide secure access to vehicles    -   Command Control Center        -   i. Integration with Infrastructure Providers

Here are some of the modules, components, or objects used or monitoredin our system: V2V (vehicle to vehicle), GPS (Global PositioningSystem), V2I (vehicle to infrastructure), HV (host vehicle), RV (remotevehicle, other vehicle, or 3^(rd) party), and active and passive safetycontrols.

FIG. 12 is for one embodiment of the invention related to virtualboundaries and clustering vehicles, to find the location and width ofthe lanes, with virtual boundaries. FIG. 13 is for one embodiment of theinvention related to current and history of data for vehicles, forprevious times, tk to t_(k-n), tracking the vehicles, e.g. with snapshots in time, in a sequence of locations.

FIG. 14 is for one embodiment of the invention related to clustering,distances between clusters (e.g. center to center, D_(cc)) (as amultiple integer (K) of a lane width (W)), and statistical distributionsfor vehicles (to distinguish the clusters, based on distributioncurve/statistics, e.g. normal distribution, of the coordinates ofvehicles' positions, at various time intervals). So, we have: D_(cc)=K W

wherein K is a positive integer (as 1, 2, 3, 4, . . . ). Even with 2lanes, we have 2 clusters, and one D_(cc) value. Thus, we can get thevalue for W (with K=1). The more lanes and more clusters (and cars), themore accurate the value for W.

FIG. 15 is for one embodiment of the invention, for a system for lanedetermination, based on path history, virtual boundary, maps, GPS, andclustering analysis, determination, and distance measurements. FIG. 16is for one embodiment of the invention, for a system for clustering,based on statistical analysis, distance measurements, and history, e.g.matching and setting the center of the corresponding cluster with thelocation of peak of the statistical curve in FIG. 14, in each of the 2dimensional axes, for X and Y coordinates. This gives us the 2coordinates of the cluster center for each cluster. Then, from thosecoordinates, the distances between the centers of the 2 clusters can beobtained, in each direction or axis, as a subtraction or difference ofvalues, which yields the width of a lane, in one of those 2 directions.

FIG. 17 is for one embodiment of the invention, for a system forclustering, based on statistical analysis, statistical distribution ofvehicles, clusters' center-to-center measurements, merging overlappingclusters (if they belong to the same cluster), edge of clusterdetermination, and coordinates of vehicles, to determine regions andlanes, as shown above.

Here, we describe a method, as one embodiment, for Lane BoundaryEstimation:

The lane boundary estimation method uses fused data from nodes(vehicles) current positions, positions history (path history), hostvehicle position and path history, host vehicle dynamics (speed, yawrate, and for some embodiments, acceleration), map database geometricalshape points and attributes, and the dynamic of the vectors that connectthe host vehicle with other remote vehicles. (See FIGS. 12-14.)

To estimate the lane boundaries locations (virtual boundaries), it isrequired to estimate the road shape, lane width, and a placementtechnique. To do that, let us look at FIG. 12 and FIG. 13, as anexample:

-   -   The map database provides very accurate representation of the        geometric shape of the road.    -   The path history can also provide a good representation of the        road geometry.    -   The vehicles (nodes) positions distribution can also provide a        good representation of the road geometry. If there are not        enough vehicles to estimate road geometry, a combined path        history and current vehicles distribution can be used to        estimate the road geometry, to extrapolate or interpolate        between them.    -   Based on the estimated geometry, the vehicles can be        grouped/clustered in each lane. This can be performed using a        straight piecewise clustering algorithm, spline-based, or an        incremental clustering algorithm. Other methods may also be        used. Basically, when the road curvature data is available, any        clustering method will be based on matching the vehicle        positions to a longitudinal grid of the road representation.        (See FIGS. 12-14.)    -   Only vehicles that their heading angle measurement (GPS        measurements) aligned with the forward road heading will have        high confidence to be a good data. The vectors can be used here,        as one example. As one example, the direction matching can be        done by dot-products of 2 vectors (V1 and V2):        V1V2 cos α

wherein α is the angle between the 2 vectors (V1 and V2). Note that forperfectly aligned vectors, we have a equal to zero, or (cos α=1) (or atmaximum value).

-   -   Once every lane cluster is established, a combination of        clusters separation distances are calculated (see FIG. 12). One        method is the following, as an example:

1—Calculate lateral distance (perpendicular to the road tangent) betweenhost lane cluster and all other lane clusters, and between all laneclusters. For example, in FIG. 12, we have the average distance betweencluster M (middle one) and cluster L (left one) (distance_ML), theaverage distance between cluster M and cluster R (right one)(distance_MR), and the average distance between clusters L and R(distance_LR).

2—Let us assume, as an example, that distance_ML=3 meter, distance_MR=4meter, and distance_LR=7.2 meter. Then, an average lane width is between3 and 4 meter. Therefore, distance_ML corresponds to one lane width,distance_MR corresponds to one lane width, and distance_LR correspond totwo lane width. Therefore, an estimated lane width can be calculated:((3+4+(7.2/2))/3)=3.53 meter. (See FIGS. 12-14.)

3—Now, we would like to establish where the virtual boundaries arelocated. The middle of the host lane is estimated (as one example) asthe line that is located at the average between the line that isgenerated from left-shifting the right cluster line by one lane widthand the line that is generated from the right-shifting the left clusterline by one lane width. (See FIGS. 12-14.)

4—Other lanes are distributed, by shifting this middle host lane by onelane width. (See FIGS. 12-14.)

5—Once middle line is established and the lane width is estimated, thevirtual lane boundary locations are estimated/found (see FIGS. 12-13).

6—The number of lanes map database attributes can also be used in theabove calculations, as one embodiment. For example, using the number oflanes limits or determines the width of the whole road, the location ofthe shoulders, and expectation of locations of the cars in differentlanes. (See FIGS. 12-14.)

Next, let us look at the Host Vehicle Position and Orientation withinthe host lane:

Now, the left and right host vehicle virtual boundaries and host vehiclemiddle lane are estimated. The host vehicle position is known.Therefore, the vehicle position with respect to the middle line and/orto the left and right boundaries can be easily calculated from the abovevalues (see FIGS. 12-13), using difference of distances or values (seeFIGS. 12-13), as they all refer to the same position or location on theroad (or on the road coordinate system), from both ways.

The heading angle of the road at the vehicle position can be calculatedfrom the road geometry estimation. Also, the vehicle heading angle isobtained from the GPS data. Therefore, the heading angle with respect tothe lane can be calculated easily by differencing the two values. Thesetwo parameters (position and heading angle with respect to the hostlane) can be used to design an LDW system, as an example.

Another method to do the estimating of these two parameters is usingmodeling and estimation. All of the above measurements, in addition tothe vector representation that connect the host vehicle with othervehicles and the host vehicle yaw rate, can be fused together (in astate model), to estimate these two main parameters (position andheading with respect to the lane). For example, we have:dD/dt=sin(Heading)*HostSpeeddHeading/dt=RoadCurvature−(HostSpeed*YawRate)dRoadCurvature/dt=0

wherein D is the distance from the middle of the host lane, Heading isthe heading or direction or angle with respect to the road,RoadCurvature is the curvature of the road, “t” is the time, HostSpeedis the speed of the host vehicle, YawRate is the rate of yaw (e.g.,related to vehicle's angular velocity, or e.g., which can be measuredwith accelerometers, in the vertical axis), and (d( )/dt) denotes thederivative of a function or a variable with respect to variable “t”.

Other models of curvature can also be used, such as the Clothoid model.For the Clothoid, e.g., as one embodiment, the curvature varies linearlywith respect to the parameter t. It is one of the simplest examples of acurve that can be constructed from its curvature. There are alsoClothoids whose curvature varies as the n-th power of the parameter t,as another embodiment.

The measurements for the above state model can be the followingparameters or set, as one example: {vector between the host vehicle andother vehicles (range and angle), curvature, heading difference,difference in position}.

Now, let us look at the advantages (comparison):

-   -   Estimating lane boundaries, when vision system does not exists,        or exists, but not fully functional.    -   In an automated system, it will be very difficult to detect and        track all lane boundaries using a vision system, due to multiple        reasons: limited Field of View (FOV) coverage, difficulty seeing        lane marking in high traffic scenario, or challenges facing        vision system in different environment conditions (e.g., poor        lane marking, challenging weather, such as ice, snow, or leaves,        challenging lighting conditions, upcoming curves at nights, or        the like).    -   Poor availability of LDW system in the above conditions, stated        in the section above.    -   V2V active safety systems/ADAS are for vehicle to vehicle        threat, and not intended for road attribute threats, such as        drifting away in your lane, as in LDW system.

As shown above, the advantages of our methods are very clear over whatthe current state-of-the-art is, e.g. using vision systems.

In this disclosure, any computing device, such as processor,microprocessor(s), computer, PC, pad, laptop, server, server farm,multi-cores, telephone, mobile device, smart glass, smart phone,computing system, tablet, or PDA can be used. The communication can bedone by or using sound, laser, optical, magnetic, electromagnetic,wireless, wired, antenna, pulsed, encrypted, encoded, or combination ofthe above. The vehicles can be car, sedan, truck, bus, pickup truck,SUV, tractor, agricultural machinery, entertainment vehicles,motorcycle, bike, bicycle, hybrid, or the like. The roads can beone-lane county road, divided highway, boulevard, multi-lane road,one-way road, two-way road, or city street. Any variations of the aboveteachings are also intended to be covered by this patent application.

The invention claimed is:
 1. A method for lane boundary estimation andhost vehicle position and orientation estimation, said methodcomprising: a global positioning system transmitting position of one ormore vehicles to a path history module; said path history modulegenerating history of coordinates of said one or more vehicles; a laneboundary estimation module generating statistical distribution curvesrepresenting said history of coordinates of said one or more vehicles ineach of two coordinates in two-dimensional plane; a clustering modulegenerating clusters based on said statistical distribution curvesrepresenting said history of coordinates of said one or more vehicles; alane determination module setting width and position of each one or morelanes lane in said road based on said clusters; a processor combiningresults of said width and said position of each lane the one or morelanes in said road, to get an overall width and position of all of theone or more lanes in said road; and estimating host vehicle positionparameter and heading parameter with respect to one of the one or morelanes lane of a said road, comprising: wherein said host vehicle headingparameter is an angle with respect to said road's direction; settingdifferential of a first distance with respect to time equal to sinefunction of said host vehicle heading parameter times speed of the hostvehicle; wherein said first distance is a distance from middle of a hostlane, corresponding to said host vehicle; setting differential of saidhost vehicle heading parameter with respect to time equal to a curvatureof said road minus product of said speed of said host vehicle times rateof yaw for said host vehicle's angular velocity, with respect tovertical axis; setting differential of said curvature of said road withrespect to time equal to zero.
 2. The method as recited in claim 1, saidmethod comprising: averaging results of said width of the one or morelanes in said road.
 3. The method as recited in claim 1, said methodcomprising: averaging results of said position of the one or more lanesin said road.
 4. The method as recited in claim 1, said methodcomprising: weighted-averaging results of said width of the one or morelanes in said road.
 5. The method as recited in claim 1, said methodcomprising: weighted-averaging results of said position of the one ormore lanes in said road.
 6. The method as recited in claim 1, saidmethod comprising: centering said clusters based on said statisticaldistribution curves.
 7. The method as recited in claim 1, said methodcomprising: determining virtual boundary line.
 8. The method as recitedin claim 1, said method comprising: recording and storing said historyof coordinates of said one or more vehicles.
 9. The method as recited inclaim 1, said method comprising: generating said clusters based on asingle vehicle.
 10. The method as recited in claim 1, said methodcomprising: generating said clusters based on multiple vehicles.
 11. Themethod as recited in claim 1, said method comprising: generating saidclusters based on a single vehicle in a single lane.
 12. The method asrecited in claim 1, said method comprising: generating said clustersbased on multiple vehicles in a single lane.
 13. The method as recitedin claim 1, said method comprising: generating said clusters based on asingle vehicle in multiple lanes.
 14. The method as recited in claim 1,said method comprising: generating said clusters based on multiplevehicles in multiple lanes.
 15. The method as recited in claim 1, saidmethod comprising: upon a lane change event, giving warning for saidlane change event.
 16. The method as recited in claim 1, said methodcomprising: upon a lane change event, giving warning for said lanechange event with sound.
 17. The method as recited in claim 1, saidmethod comprising: upon a lane change event, giving warning for saidlane change event with light.
 18. The method as recited in claim 1, saidmethod comprising: using a road map.
 19. The method as recited in claim1, said method comprising: measuring distance between said clusters. 20.The method as recited in claim 1, said method comprising: measuringdistance between centers of said clusters.
 21. The method as recited inclaim 1, said method comprising: said lane determination module usingmap attributes.
 22. The method as recited in claim 1, said methodcomprising: said lane determination module using shape points.
 23. Themethod as recited in claim 1, said method comprising: said lanedetermination module using number of lanes.
 24. The method as recited inclaim 1, said method comprising: said lane determination module usingroad structure.
 25. The method as recited in claim 1, said methodcomprising: matching the host vehicle position to longitudinal grid ofroad representation.
 26. The method as recited in claim 1, said methodcomprising: determining a curvature using Clothoid model.